import sys
sys.path.insert(0, "../scripts")

import cv2
import json
import rplidar
import time
import numpy as np


with open("../log/radar.json") as f:
    datas = f.readlines()
    for line in datas:
        if line == "\n": continue
        
        data = json.loads(line)
        # print(data)
        res = rplidar.find_circle4(data["range"])
        d = rplidar.calc_dist(res[0], res[1])

        # print(data)
        print(res, d)
        print()
# candidate = []

# with open("./res/radar_grab.json") as f:
#     datas = np.array(json.load(f)) * 100

#     for i in range(20):
#         data = datas[50 + i]
#         rplidar.range_filter_rect(data, 400, 300)
#         res = rplidar.find_circle3(np.array(data))

#         print(res)
#         if res is not None:
#             print(rplidar.calc_dist(res[0], res[1]))

#             if len(candidate) == 0:
#                 candidate.append([res, -1])
            
#             believable = False
#             for cand in candidate:
#                 var = np.var(cand[0] - res)
#                 print(var)
#                 if var < 8:
#                     cand[1] += 1
#                     cand[0] = (cand[0] + res) / 2
#                     believable = True
            
#             if not believable:
#                 candidate.append([res, 0])
            
#             print(candidate)
#             print()

#     candidate.sort(key = lambda c : c[1], reverse=True)
#     print(candidate[0])
#     res = candidate[0][0]
#     p0 = res[0]
#     p1 = res[1]
#     print(p0, p1, rplidar.calc_dist(p0, p1))


        # img = rplidar.map2img(data, 400)
        # cv2.circle(img, p0, 5, 255)
        # cv2.circle(img, p1, 5, 255)
        # cv2.imshow("data", img)
        
        # c = cv2.waitKey(1) & 0xff
        # if c == 27:
        #     # 释放所有窗口
        #     cv2.destroyAllWindows()
        #     break

        # time.sleep(0.1)
# 50 .. 60


# dist     index   diff
# 2.5m,     5,     2cm
# 1m        12,    2cm